#include "ros/ros.h"
#include <geometry_msgs/Twist.h>         // 指线速度角速度的消息类型
#include <geometry_msgs/Point.h>         // 位置信息消息类型
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>       // 用于坐标转换
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>           // 用于读取里程计信息
#include <robust_formation/FormationCtrl.h> 
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>
// inlcude iostream and string libraries
#include <iostream>
#include <map>
#include <math.h>
#include <string>
#include <vector>

using namespace std;
using namespace Eigen;

constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

/********************/
/* CLASS DEFINITION */
/********************/
// 这一版本的formationctrl添加了多机器人编队的队形控制 yzg v2.0.1
// 在上一版本的基础上，队形变化增加了条形和三角形，并控制在两种队形的情况下的距离 yzg v2.0.2
class FormationCtrlNode
{
public:
    FormationCtrlNode();
    ~FormationCtrlNode();
    int get_thread_numbers();
private:
    ros::NodeHandle _nh;
    ros::Subscriber _sub_leader_odom, _sub_other_robot_odom, _sub_robot_odom, _sub_cmd;
    ros::Publisher _pub_formation_cmd, _pub_show_formation_target;
    ros::Timer _timer1;

    nav_msgs::Odometry _leader_odom, _other_robot_odom, _this_robot_odom;
    geometry_msgs::Twist _sub_twist_cmd;
    geometry_msgs::Point _pub_formation_target;
    geometry_msgs::PointStamped _show_formation_target;

    // 当前跟随机器人的命名空间
    string _follower_frame;
    string _simulation;

    Formation_MPC _mpc;

    double _cmd_distance, _cmd_angle;   // control parameters
    double _initial_len,_initial_angle;
    double _dt;
    int _controller_freq, _thread_numbers;

    void leader_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void other_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void this_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void read_cmd(const geometry_msgs::Twist::ConstPtr& cmd);  // 读取用户输入的命令

    void controlLoopCB(const ros::TimerEvent&);  // 控制回调函数
};

FormationCtrlNode::FormationCtrlNode() {
    ROS_INFO("===============检查点1: 进入FormationCtrlNode===============");
    ros::NodeHandle pn("~");
    pn.param("thread_numbers", _thread_numbers, 2);     // number of threads for this ROS node
    pn.param("controller_freq", _controller_freq, 4);   // controller_frequence
    _dt = double(1.0/_controller_freq);                 // time step duration dt in s

    //Parameter for topics & Frame name
    pn.param<std::string>("follower_frame", _follower_frame, "tb3_1" );
    pn.param<std::string>("simulation", _simulation, "yes" );
    _sub_leader_odom = _nh.subscribe("/tb3_0/odom", 1, &FormationCtrlNode::leader_odom_CB, this);
    // _sub_other_robot_odom = _nh.subscribe("/tb3_2/odom", 1, &FormationCtrlNode::other_robot_odom_CB, this);
    // _sub_robot_odom = _nh.subscribe("/tb3_1/odom", 1, &FormationCtrlNode::this_robot_odom_CB, this);
    // _pub_formation_cmd = _nh.advertise<geometry_msgs::Point>("tb3_1/formation_target", 1);

    if (_follower_frame == "tb3_1") {
        if (_simulation == "yes") {
            _sub_other_robot_odom = _nh.subscribe("/tb3_2/odom", 1, &FormationCtrlNode::other_robot_odom_CB, this);
            _sub_robot_odom = _nh.subscribe("/tb3_1/odom", 1, &FormationCtrlNode::this_robot_odom_CB, this);
        } else if (_simulation == "no") {
            _sub_other_robot_odom = _nh.subscribe("/tb3_2/fake_odom", 1, &FormationCtrlNode::other_robot_odom_CB, this);
            _sub_robot_odom = _nh.subscribe("/tb3_1/fake_odom", 1, &FormationCtrlNode::this_robot_odom_CB, this);
        }
        _cmd_angle = deg2rad(45);
    } else {
        if (_simulation == "yes") {
            std::cout << "**********************************simulation**********************************" << std::endl;
            _sub_other_robot_odom = _nh.subscribe("/tb3_1/odom", 1, &FormationCtrlNode::other_robot_odom_CB, this);
            _sub_robot_odom = _nh.subscribe("/tb3_2/odom", 1, &FormationCtrlNode::this_robot_odom_CB, this);
        } else if (_simulation == "no") {
            std::cout << "**********************************reality**********************************" << std::endl;
            _sub_other_robot_odom = _nh.subscribe("/tb3_1/fake_odom", 1, &FormationCtrlNode::other_robot_odom_CB, this);
            _sub_robot_odom = _nh.subscribe("/tb3_2/fake_odom", 1, &FormationCtrlNode::this_robot_odom_CB, this);
        }
        _cmd_angle = deg2rad(135);
    }
    _sub_cmd = _nh.subscribe("/cmd_user", 1, &FormationCtrlNode::read_cmd, this);
    _pub_formation_cmd = _nh.advertise<geometry_msgs::Point>("/formation_target", 1);
    _pub_show_formation_target = _nh.advertise<geometry_msgs::PointStamped>("/show_formation_target", 1);

    //Timer
    _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &FormationCtrlNode::controlLoopCB, this); // 4Hz

    _cmd_distance = sqrt(2);                                //初始距离
    _initial_len = 1.0;
    _initial_angle = deg2rad(15);                           //一次底角变化十五度
}

FormationCtrlNode::~FormationCtrlNode() {
    // ~FormationCtrlNode()
}

void FormationCtrlNode::controlLoopCB(const ros::TimerEvent&) {
    ROS_INFO("===============检查点2: controlLoopCB===============");
        

    
    // 检查是否正在被其他线程使用
    static bool in_use = false;
    if (in_use) {
        ROS_WARN("⚠️ MPC求解器正忙，跳过本次调用");
        return;
    }
    in_use = true;

    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    double distance_gain = cmd_user.linear.y;
    double distance_bigchange_angle = cmd_user.angular.x;       //引入队形角度变化命令
    double distance_gain_angle = cmd_user.angular.y;            //引入角度缩小放大
    nav_msgs::Odometry other_robot_odom = _other_robot_odom;
    nav_msgs::Odometry leader_robot_odom = _leader_odom;
    nav_msgs::Odometry this_robot_odom = _this_robot_odom;
    double other_robot_pos_x = other_robot_odom.pose.pose.position.x;
    double other_robot_pos_y = other_robot_odom.pose.pose.position.y;
    double leader_robot_pos_x = leader_robot_odom.pose.pose.position.x;
    double leader_robot_pos_y = leader_robot_odom.pose.pose.position.y;
    double this_robot_pos_x = this_robot_odom.pose.pose.position.x;
    double this_robot_pos_y = this_robot_odom.pose.pose.position.y;
    tf::Pose pose;
    tf::poseMsgToTF(this_robot_odom.pose.pose, pose);
    const double theta = tf::getYaw(pose.getRotation());
    // 输入量、参考量
    double ref_distance = (_initial_len + distance_gain) * sqrt(2);
    double ref_angle = _cmd_angle;
            
    if (_follower_frame == "tb3_1") {
        //左边机器人参数设置
          if(distance_bigchange_angle){
            //条型的设置
            // ref_distance = 0.5 * sqrt(2);
            ref_distance = 0.8 * (_initial_len + distance_gain) * sqrt(2);  //设置变成条形队形之后的队形距离，左边机器人为右边机器人的距离一半
            ref_angle = deg2rad(90);                                        //角度都为90度
          }
          else{         
            //三角形的设置
            ref_angle = _cmd_angle + _initial_angle*distance_gain_angle;
          }
        }
    else{             
        //右边机器人参数设置                  
          if(distance_bigchange_angle){
            //变化成条形设置
            ref_distance = 1.6 * (_initial_len + distance_gain) * sqrt(2);
            ref_angle = deg2rad(90);
          }
          else{//右边角
            ref_angle = deg2rad(135) - _initial_angle*distance_gain_angle;
          }
        }



    VectorXd vec_cmd(6);
    vec_cmd << ref_distance, ref_angle, leader_robot_pos_x, leader_robot_pos_y, other_robot_pos_x, other_robot_pos_y;

    // 给定一个初始状态
    VectorXd state(2);
    state << ref_distance, ref_angle;

    // Solve MPC Problem
  
    vector<double> mpc_results = _mpc.Solve(state, vec_cmd);
    in_use = false;
    
    double target_x = leader_robot_pos_x - mpc_results[0] * sin(mpc_results[1]);
    double target_y = leader_robot_pos_y - mpc_results[0] * cos(mpc_results[1]);

    // Convert to the vehicle coordinate system
    const double dx = target_x - this_robot_pos_x;
    const double dy = target_y - this_robot_pos_y;
    _pub_formation_target = geometry_msgs::Point();
    _pub_formation_target.x = dx * cos(theta) + dy * sin(theta);
    _pub_formation_target.y = dy * cos(theta) - dx * sin(theta);
    _pub_formation_cmd.publish(_pub_formation_target);

    geometry_msgs::Point temp_point = geometry_msgs::Point();
    temp_point.x = target_x;
    temp_point.y = target_y;
    _show_formation_target.header.frame_id = "/tb3_1/odom";
    _show_formation_target.header.stamp = ros::Time::now();
    _show_formation_target.point = temp_point;
    _pub_show_formation_target.publish(_show_formation_target);

    double distance = pow((other_robot_pos_x - target_x), 2) + pow((other_robot_pos_y - target_y), 2);

    // print
    if (_simulation == "yes") {
        std::cout << "**********************************simulation**********************************" << std::endl;
    } else if (_simulation == "no") {
        std::cout << "**********************************reality**********************************" << std::endl;
    }
    std::cout << "this_robot_pos_x: " << this_robot_pos_x << " this_robot_pos_y: " << this_robot_pos_y << std::endl;
    std::cout << "other_robot_pos_x: " << other_robot_pos_x << " other_robot_pos_y: " << other_robot_pos_y << std::endl;
    std::cout << std::endl;
    std::cout << "ref_distance: " << ref_distance << " ref_angle: " << ref_angle << std::endl;
    std::cout << "l: " << mpc_results[0] << " theta: " << mpc_results[1] << std::endl;
    std::cout << "solved target_x: " << target_x << " solved target_y: " << target_y << std::endl;
    std::cout << "distance: " << distance << std::endl;
    std::cout << "veh_X: " << _pub_formation_target.x << "veh_Y: " << _pub_formation_target.y << std::endl;
}

int FormationCtrlNode::get_thread_numbers() {
    ROS_INFO("===============检查点3: get_thread_numbers===============");
    return _thread_numbers;
}

void FormationCtrlNode::leader_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg) {
    ROS_INFO("===============检查点4: leader_odom_CB===============");
    _leader_odom = *odomMsg;
}

void FormationCtrlNode::other_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg) {
    ROS_INFO("===============检查点5: other_robot_odom_CB===============");
    _other_robot_odom = *odomMsg;
}

void FormationCtrlNode::this_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg) {
    _this_robot_odom = *odomMsg;
}

void FormationCtrlNode::read_cmd(const geometry_msgs::Twist::ConstPtr& cmd) {
    ROS_INFO("===============检查点6: read_cmd===============");
    _sub_twist_cmd = *cmd;
}

/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ROS_INFO("===============检查点0: init===============");
    ros::init(argc, argv, "FormationCtrlNode");

    FormationCtrlNode formation_controler_node;

    ros::AsyncSpinner spinner(formation_controler_node.get_thread_numbers()); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}
